/**************************************************************************************************
* Title: SumPlane_16f.cpp
* Author: Gael Huber
* Description: Describes functions for 16-byte aligned plane
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Normalize plane (so that |a,b,c| == 1
**************************************************************************************************/
Plane_16f* PlaneNormalize_16f(Plane_16f* pOut, const Plane_16f* pP) {
	__m128 m(_mm_mul_ps(pP->v, pP->v));
	m = _mm_set1_ps(1.0f / sqrtf(m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + m.m128_f32[3]));
	pOut->v = _mm_mul_ps(pP->v, m);
	return pOut;
}

/**************************************************************************************************
* Find intersection point between a plane and a line. If the line is parallel to the plane, null is 
* returned.
**************************************************************************************************/
Vector3_16f* PlaneIntersectLine_16f(Vector3_16f* pOut, const Plane_16f* pP, const Vector3_16f* pV1, 
	const Vector3_16f* pV2) {
	// Get the vector normal
	Vector3_16f n(pP->v);

	// Get the vector representing the length of the line segment
	Vector3_16f d;
	Vec3Sub_16f(&d, pV2, pV1);

	// Find the dot product of dn
	float dn(Vec3Dot_16f(&d, &n));

	// If dn == 0, return 0
	if(dn == 0.0f) {
		pOut = 0;
	} else {
		// dn acts as denominator for finding intersection point, so we must now find the numerator.
		// From this we can find the modifier amount as well
		dn = (pP->d - Vec3Dot_16f(pV1, &n)) / dn;
		
		// pOut = pV1 + td
		Vec3Scale_16f(&d, &d, dn);
		Vec3Add_16f(pOut, pV1, &d);
	}

	return pOut;
}

/**************************************************************************************************
* Construct a plane from a point and a normal
**************************************************************************************************/
Plane_16f* PlaneFromPointNormal_16f(Plane_16f* pOut, const Vector3_16f* pPoint, const Vector3_16f* pNormal) {
	// Generally speaking, d = p*n for any point on the plane
	pOut->v = pNormal->v;
	pOut->d = -Vec3Dot_16f(pPoint, pNormal);
	return pOut;
}

/**************************************************************************************************
* Construct a plane from 3 points
**************************************************************************************************/
Plane_16f* PlaneFromPoints_16f(Plane_16f* pOut, const Vector3_16f* pV1, const Vector3_16f* pV2, 
	const Vector3_16f* pV3) {
	Vector3_16f c, e1, e2;

	// e1 = v2 - v1
	Vec3Sub_16f(&e1, pV2, pV1);

	// e2 = v3 - v1
	Vec3Sub_16f(&e2, pV3, pV1);

	// Compute the cross product
	Vec3Cross_16f(&c, &e2, &e1);

	// n = c / |c|
	Vector3_16f n(c / Vec3Length_16f(&c));

	// From here, use point normal form
	return PlaneFromPointNormal_16f(pOut, pV1, &n);
}

/**************************************************************************************************
* Transform a plane by a matrix. The vector (a,b,c) must be normal. M should be the inverse 
* transpose of the transformation desired.
**************************************************************************************************/
Plane_16f* PlaneTransform_16f(Plane_16f* pOut, const Plane_16f* pP, const Matrix_16f* pM) {
	// Interpret the plane as a homogeneous vector
	Vector4_16f p(pP->v);

	// Transform the plane
	Vector4_16f pT;
	Vec4Transform_16f(&pT, &p, pM);

	// Reinterpret pT to a plane
	pOut->v = pT.v;
	return pOut;
}